/*
 * ArduinoNunchuk Demo
 * 
 * Copyright 2011-2012 Gabriel Bianconi, http://www.gabrielbianconi.com/
 *
 * Project URL: http://www.gabrielbianconi.com/projects/arduinonunchuk/
 * 
 */

//#include <LiquidCrystal.h>
#include <Wire.h>
#include <ArduinoNunchuk.h>
#include <AFMotor.h>

#define BAUDRATE 19200
#define MIN_THRESHOLD 50
//#define RUSTY

AF_DCMotor leftRear(3, MOTOR34_1KHZ);
AF_DCMotor rightRear(4, MOTOR12_1KHZ);
AF_DCMotor rightFront(2, MOTOR34_1KHZ);
AF_DCMotor leftFront(1, MOTOR12_1KHZ);
AF_DCMotor motors[4] = {leftRear, rightRear, rightFront, leftFront};

ArduinoNunchuk nunchuk = ArduinoNunchuk();

//LiquidCrystal lcd(22,23,24,25,26,27);

int xV;
int yV;
int x;
int y;
int accx;
int	accy;
int zbut;
int cbut;
int right;
int left;

void setup()
{
  allStop();
  Serial.begin(BAUDRATE);
  nunchuk.init();
  //lcd.begin(16,2); 
}

void loop()
{
static int ret;

    ret = nunchuk.update();

	if(ret)
	{
		//lcd.clear();
		//lcd.print("NunChuck Connnected!");
		xV = nunchuk.analogX;
		yV = nunchuk.analogY;
		accx = nunchuk.accelX;
		accy = nunchuk.accelY;
		zbut = nunchuk.zButton;
		cbut = nunchuk.cButton; 
		//Setup contraints and maps to scale everything to what we need
		// for the motors
		//    xV = constrain(xV, 60, 1100);
		//    yV = constrain(yV, 0, 1075);
		if(zbut)
		{
			accx = constrain(accx , 280, 680);
			accy = constrain(accy, 280, 680);
			x = map(accx, 280, 680, -255, 255);
			y = map(accy, 280, 680, -255, 255);
		}
		else
		{
			xV = constrain(xV, 30, 220);
			yV = constrain(yV, 30, 220);
			x = map(xV, 30, 220, -255, 255);
			y = map(yV, 30, 220, -255, 255);
		}
		mapSpeeds();

		//debug();
		setLeftSpeed(left);
		setRightSpeed(right);

#ifdef RUSTY  
  Serial.print(nunchuk.analogX, DEC);
  Serial.print(' ');
  Serial.print(nunchuk.analogY, DEC);
  Serial.print(' ');
  Serial.print(nunchuk.accelX, DEC);
  Serial.print(' ');
  Serial.print(nunchuk.accelY, DEC);
  Serial.print(' ');
  Serial.print(nunchuk.accelZ, DEC);
  Serial.print(' ');
  Serial.print(nunchuk.zButton, DEC);
  Serial.print(' ');
  Serial.println(nunchuk.cButton, DEC);
#endif
	}
	else
	{
		//lcd.clear(); 
		//lcd.print("NunChuck not connected");
	}

	delay(100);
}

void mapSpeeds() {
  left = 0;
  right = 0;
  int dir = (y>0) ? 1 : -1;
  if (abs(y) >= abs(x/2)) {
    left = y;
    right = y;
    if (x>0) {
      // decrease right wheels speed
      right -= (dir * x);
      right = (dir>0) ? max(right, 0) : min(right, 0);
      // increase left speed a bit if turning in an arc
      left += (dir * (x/2));
      left = (dir>0) ? min(left, 255) : max(left, -255);
    } else {
      // decrease left wheels speed
      left -= (dir * -x);
      left = (dir>0) ? max(left, 0) : min(left, 0);
      // increase right speed a bit if turning in an arc
      right += (dir * (-x/2));
      right = (dir>0) ? min(right, 255) : max(right, -255);
    }
  } else {
    left = x;
    right = -x;
  }
}

void setLeftSpeed(int s) 
{
  if (abs(s) < MIN_THRESHOLD) 
  {
    leftFront.setSpeed(0);
    leftRear.setSpeed(0);
    leftFront.run(RELEASE);
    leftRear.run(RELEASE);
  } 
  else 
  {
    if (s > 0) 
	{
      leftFront.run(FORWARD);
      leftRear.run(FORWARD);
    } 
	else 
	{
      leftFront.run(BACKWARD);
      leftRear.run(BACKWARD);
    }
    s = abs(s);
    if (s > 250) 
	{
      s = 255;
    }
    leftFront.setSpeed(s);
    leftRear.setSpeed(s);
  }
}

void setRightSpeed(int s) {
  if (abs(s) < MIN_THRESHOLD) {
    rightFront.setSpeed(0);
    rightRear.setSpeed(0);
    rightFront.run(RELEASE);
    rightRear.run(RELEASE);
  } else {
    if (s > 0) {
      rightFront.run(FORWARD);
      rightRear.run(FORWARD);
    } else {
      rightFront.run(BACKWARD);
      rightRear.run(BACKWARD);
    }
    s = abs(s);
    if (s > 250) {
      s = 255;
    }
    rightFront.setSpeed(s);
    rightRear.setSpeed(s);
  }
}


void allStop() {
  for(int m=0;m<4;m++) {
    motors[m].setSpeed(0);
    motors[m].run(RELEASE);
  }
}

void debug() {
    Serial.print("x = ");
    Serial.print(x);
    Serial.print("  y = ");
    Serial.print(y);

    Serial.print("   left = ");
    Serial.print(left);
    Serial.print("  right = ");
    Serial.print(right);

    Serial.println("");
    Serial.println("");
}
